import cv2
import numpy as np
from std_msgs.msg import ColorRGBA
from std_msgs.msg import Int8
from cv_bridge import CvBridge, CvBridgeError
import rospy
from sensor_msgs.msg import Image, RegionOfInterest


def resize(img,heigth = None,width = None):
    rate = None
    dim = None
    h,w = img.shape[:2]
    if heigth is None:
        rate = width/float(w)
        heigth = int(h * rate)
        dim = (width,heigth)
    if width is None:
        rate = heigth/float(h)
        width = int(w * rate)
        dim = (width,heigth)
    size = cv2.resize(img,dim,interpolation=cv2.INTER_AREA)
    return size


def image_callback(data):
    global ros_image
    # 使用cv_bridge将ROS的图像数据转换成OpenCV的图像格式
    bridge = CvBridge()
    try:
        ros_image = bridge.imgmsg_to_cv2(data, "bgr8")
    # cv2.imshow("test", ros_image)
    except CvBridgeError as e:
        print(e)



def find_Circle(frame_gray, frame, mi=1, ma=5000, flag=False):
    circles = cv2.HoughCircles(frame_gray, cv2.HOUGH_GRADIENT, 1, 90, param1=100, param2=25, minRadius=mi, maxRadius=ma)
    number = []
    if circles is not None:
        circles = np.uint16(np.around(circles))
        #print(circles[0])

        for i in circles[0, :]:
            cv2.circle(frame, (i[0], i[1]), i[2], (0, 0, 255), 2)
            cv2.circle(frame, (i[0], i[1]), 2, (0, 0, 255), -1)
            str1 = '(' + str(i[0]) + ',' + str(i[1]) + ')'
            # cv2.putText(frame, str1, (i[0] + 10, i[1] + 10),
            # cv2.FONT_HERSHEY_COMPLEX, 0.6, (0, 0, 0), 1)
            cv2.putText(frame, str1, (i[0] + 10, i[1] + 10),
                        cv2.FONT_HERSHEY_COMPLEX, 0.6, (0, 0, 255), 1)
            if flag:
                print("({}, {})".format(i[0], i[1]))
            number = [i[0], i[1], i[2]]
            n = 1
    if len(number) == 0:
        print("没有发现圆")
    return number


lhsv = np.array([35, 43, 46])
hhsv = np.array([77, 255, 255])
center = [160, 284]
rospy.init_node('vision', anonymous=True)
vision_data=ColorRGBA()
vision_pub = rospy.Publisher('/vision_data',ColorRGBA, queue_size=10)
b = 0
r = 0
g = 0
a = 0
ros_image = np.zeros((480, 640, 3), np.uint8)
ros_image.fill(255)
image_sub = rospy.Subscriber("/iris_0/camera/image_raw", Image, image_callback, queue_size=10)
while not rospy.is_shutdown():
    frame = ros_image
    frame = resize(frame, heigth=320)
    print(frame.shape)
    frame = cv2.GaussianBlur(frame, (5, 5), 0)
    frame_hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
    mask = cv2.inRange(frame_hsv, lhsv, hhsv)
    Canny = cv2.Canny(mask, 20, 50)
    number = find_Circle(Canny, frame, mi=5, ma=600)
    if len(number) >= 2:
        x_error = number[0] - center[1]
        y_error = number[1] - center[0]
        r = -x_error
        g = -y_error
    cv2.imshow("frame", frame)
    cv2.imshow("mask", mask)
    cv2.imshow("Canny", Canny)
    if cv2.waitKey(1) == ord(' '):
        break
    vision_data.r = r
    vision_data.g = g
    vision_data.b = b
    vision_data.a = a
    print("{} {} {} {}".format(r,g,b,a))
    vision_pub.publish(vision_data)
cv2.destroyAllWindows()